#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>

double my_get_otsu_thresh(const cv::Mat& mat) {
    CV_Assert(mat.type() == CV_8UC1);
    
    int rows = mat.rows;
    int cols = mat.cols;
    
    const int L = 256;  // 256 gray levels, 0, 1, 2, ..., 255
    int N = rows * cols;
    int n_i[L] = {0};  // number of points for each gray level
    for (int i = 0; i < rows; i++) {
        const uchar* p = mat.ptr<uchar>(i);
        for (int j = 0; j < cols; j++) {
            n_i[p[j]]++;  // for corresponding level, number of points plus 1
        }
    }
    
    double mu_T = 0;
    for (int i = 0; i < L; i++) {
        mu_T += i * (double)n_i[i];
    }
    mu_T /= N;
    
    double mu_k = 0, omega_k = 0;
    double max_sigma_B = 0, k_star = 0;
    for (int i = 0; i < L; i++) {
        double p_i, i_p_i;
        p_i = n_i[i] * 1.0 / N;
        i_p_i = n_i[i] * 1.0 * i / N;
        omega_k += p_i;
        mu_k += i_p_i;
        if (std::min(omega_k, 1. - omega_k) < FLT_EPSILON || std::max(omega_k, 1. - omega_k) > 1. - FLT_EPSILON) {
            continue;
        }
        
        double sigma_B = (mu_T * omega_k - mu_k) * (mu_T * omega_k - mu_k) / (omega_k * (1. - omega_k));
        if (sigma_B > max_sigma_B) {
            max_sigma_B = sigma_B;
            k_star = i;
        }
    }
    return k_star;
}
        
double my_get_triangle_thresh(const cv::Mat& mat) {
    CV_Assert(mat.type() == CV_8UC1);
    
    int rows = mat.rows;
    int cols = mat.cols;
    
    const int L = 256;
    int n_i[L] = {0};
    for (int i = 0; i < rows; i++) {
        const uchar* p = mat.ptr<uchar>(i);
        for (int j = 0; j < cols; j++) {
            n_i[p[j]]++;
        }
    }
    
    int left = 0, right = 0, max_index = 0, max_val = 0;
    bool isFlipped = false;
    
    // from histogram, find the first left one where is 0
    for (int i = 0; i < L; i++) {
        if (n_i[i] > 0) {
            left = (i == 0 ? 0 : i-1);
            break;
        }
    }
        
    // find the right
    for (int i = L-1; i > 0; i--) {
        if (n_i[i] > 0) {
            right = (i == L-1 ? L-1 : i+1);
            break;
        }
    }
        
    // find max value
    for (int i = 0; i < L; i++) {
        if (n_i[i] > max_val) {
            max_val = n_i[i];
            max_index = i;
        }
    }
        
    // check if need to flip
    if (max_index - left < right - max_index) {  // max value is closer to left
        isFlipped = true;
        int i = 0, j = L-1;
        while (i < j) {
            int temp = n_i[i];
            n_i[i] = n_i[j];
            n_i[j] = temp;
            i++;
            j--;
        }
        left = L - 1 - right;
        max_index = L - 1 - max_index;
    }
        
    double thresh = left;
    double a = max_val, b = max_index - left;
    
    double dist = 0;
    for (int i = left+1; i <= max_index; i++) {
        double tempdist = a * i - b * n_i[i];
        if (tempdist > dist) {
            dist = tempdist;
            thresh = i;
        }
    }
    
    /*
    double c = std::sqrt(a*a + b*b);
    double d = a*b/c;
    double e = std::sqrt(b*b-d*d);
    double i_thresh = e * b / c + left;
    thresh = (int)i_thresh;
    */
    thresh--;
    
    if (isFlipped) {
        thresh = L - 1 - thresh;
    }
    return thresh;
}
    
cv::Mat my_adaptive_thresh_by_mean_c(const cv::Mat& mat, double max_val, int block_size, double C) {
    CV_Assert(mat.type() == CV_8UC1);
    CV_Assert(block_size > 1 && block_size % 2 == 1);
    CV_Assert(max_val >= 1 && max_val <= 255);
    
    cv::Mat matMean;
    cv::blur(mat, matMean, cv::Size(block_size, block_size), cv::Point(-1, -1), cv::BORDER_REPLICATE | cv::BORDER_ISOLATED);
    cv::Mat dst(mat.size(), CV_8UC1);
    for (int i = 0; i < mat.rows; i++) {
         const uchar* p = mat.ptr<uchar>(i);
         const uchar* pp = matMean.ptr<uchar>(i);
         uchar* ppp = dst.ptr<uchar>(i);
         for (int j = 0; j < mat.cols; j++) {
             ppp[j] = (p[j] > pp[j] - C ? (int)max_val : 0);
        }
    }
    return dst;
}
         
cv::Mat my_adaptive_thresh_by_gaussian_c(const cv::Mat& mat, double max_val, int block_size, double C) {
    CV_Assert(mat.type() == CV_8UC1);
    CV_Assert(block_size > 1 && block_size % 2 == 1);
    CV_Assert(max_val >= 1 && max_val <= 255);
    
    cv::Mat matDouble;
    mat.convertTo(matDouble, CV_64FC1);
    cv::Mat matMeanDouble;
    cv::GaussianBlur(matDouble, matMeanDouble, cv::Size(block_size, block_size), 0, 0, cv::BORDER_REPLICATE | cv::BORDER_ISOLATED);
    cv::Mat matMean;
    matMeanDouble.convertTo(matMean, mat.type());
    cv::Mat dst(mat.size(), CV_8UC1);
    for (int i = 0; i < mat.rows; i++) {
         const uchar* p = mat.ptr<uchar>(i);
         const uchar* pp = matMean.ptr<uchar>(i);
         uchar* ppp = dst.ptr<uchar>(i);
         for (int j = 0; j < mat.cols; j++) {
             ppp[j] = (p[j] > pp[j] - C ? (int)max_val : 0);
        }
    }
    return dst;
}

int main(int argc, char **argv) {
    /*
    cv::Mat mat = (cv::Mat_<uchar>(3, 3) << 1, 1, 3, 4, 5, 6, 7, 8, 9);
    std::cout << "mat = \n" << mat << std::endl;
    
    double thresh = 5;
    double maxval = 100;
    cv::Mat binary, binary_inv;
    cv::threshold(mat, binary, thresh, maxval, cv::THRESH_BINARY);  // > 5 --> 100, <= 5 --> 0
    cv::threshold(mat, binary_inv, thresh, maxval, cv::THRESH_BINARY_INV);  // > 5 --> 0, <= 5 --> 100
    std::cout << "binary = \n" << binary << std::endl;
    std::cout << "binary_inv = \n" << binary_inv << std::endl;
    
    cv::Mat trunc, tozero, tozero_inv;
    cv::threshold(mat, trunc, thresh, maxval, cv::THRESH_TRUNC);  // > 5 --> 5, <= 5 --> keep source
    cv::threshold(mat, tozero, thresh, maxval, cv::THRESH_TOZERO);  // > 5 --> keep source, <= 5 --> 0
    cv::threshold(mat, tozero_inv, thresh, maxval, cv::THRESH_TOZERO_INV);  // > 5 --> 0, <= 5 --> keep source
    std::cout << "trunc = \n" << trunc << std::endl;
    std::cout << "tozero = \n" << tozero << std::endl;
    std::cout << "tozero_inv = \n" << tozero_inv << std::endl;
    */
    
    cv::Mat mat(15, 16, CV_8UC1);
    cv::randu(mat, cv::Scalar(0), cv::Scalar(255));
    double thresh = 5;
    double maxval = 255;
    
    /*
    cv::Mat binary_otsu, my_binary_otsu;
    double otsu_thresh = cv::threshold(mat, binary_otsu, thresh, maxval, cv::THRESH_BINARY | cv::THRESH_OTSU);
    std::cout << "otsu_thresh = " << otsu_thresh << std::endl;
    double my_otsu_thresh = my_get_otsu_thresh(mat);
    std::cout << "my_otsu_thresh = " << my_otsu_thresh << std::endl;
    cv::threshold(mat, my_binary_otsu, my_otsu_thresh, maxval, cv::THRESH_BINARY);
    std::vector<cv::Point> nonzeros;
    cv::Mat otsu_diff = my_binary_otsu != binary_otsu;
    cv::findNonZero(otsu_diff, nonzeros);
    std::cout << "nonzeros = " << nonzeros << std::endl;
    */
    
    cv::Mat binary_triangle, my_binary_triangle;
    double triangle_thresh = cv::threshold(mat, binary_triangle, thresh, maxval, cv::THRESH_BINARY | cv::THRESH_TRIANGLE);
    std::cout << "triangle_thresh = " << triangle_thresh << std::endl;
    double my_triangle_thresh = my_get_triangle_thresh(mat);
    std::cout << "my_triangle_thresh = " << my_triangle_thresh << std::endl;
    cv::threshold(mat, my_binary_triangle, my_triangle_thresh, maxval, cv::THRESH_BINARY);
    std::vector<cv::Point> nonzeros1;
    cv::Mat triangle_diff = my_binary_triangle != binary_triangle;
    cv::findNonZero(triangle_diff, nonzeros1);
    std::cout << "nonzeros1 = " << nonzeros1 << std::endl;
    
    /*
    cv::Mat mat(15, 16, CV_8UC1);
    cv::randu(mat, cv::Scalar(0), cv::Scalar(255));
    cv::Mat matAdaptiveThreshMeanC, matMyAdaptiveThreshMeanC;
    double max_value = 255;
    int block_size = 3;
    double C = 5;
    cv::adaptiveThreshold(mat, matAdaptiveThreshMeanC, max_value, cv::ADAPTIVE_THRESH_MEAN_C, cv::THRESH_BINARY, block_size, C);
    matMyAdaptiveThreshMeanC = my_adaptive_thresh_by_mean_c(mat, max_value, block_size, C);
    std::cout << "matAdaptiveThreshMeanC = \n" << matAdaptiveThreshMeanC << std::endl;
    std::cout << "matMyAdaptiveThreshMeanC = \n" << matMyAdaptiveThreshMeanC << std::endl;
    cv::Mat matAdaptiveThreshMeanCDiff = matAdaptiveThreshMeanC != matMyAdaptiveThreshMeanC;
    std::vector<cv::Point> nonzeros;
    cv::findNonZero(matAdaptiveThreshMeanCDiff, nonzeros);
    std::cout << "nonzeros = " << nonzeros << std::endl;
    
    cv::Mat matAdaptiveThreshGaussianC, matMyAdaptiveThreshGaussianC;
    cv::adaptiveThreshold(mat, matAdaptiveThreshGaussianC, max_value, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY, block_size, C);
    matMyAdaptiveThreshGaussianC = my_adaptive_thresh_by_gaussian_c(mat, max_value, block_size, C);
    std::cout << "matAdaptiveThreshGaussianC = \n" << matAdaptiveThreshGaussianC << std::endl;
    std::cout << "matMyAdaptiveThreshGaussianC = \n" << matMyAdaptiveThreshGaussianC << std::endl;
    cv::Mat matAdaptiveThreshGaussianCDiff = matAdaptiveThreshGaussianC != matMyAdaptiveThreshGaussianC;
    std::vector<cv::Point> nonzeros1;
    cv::findNonZero(matAdaptiveThreshGaussianCDiff, nonzeros1);
    std::cout << "nonzeros1 = " << nonzeros1 << std::endl;
    */
    /*
    cv::Mat mat(7, 8, CV_8UC1);
    cv::randu(mat, cv::Scalar(0), cv::Scalar(255));
    cv::Mat dst;
    cv::inRange(mat, cv::Scalar(90), cv::Scalar(150), dst);  // 90 <= (i, j) <= 150 --> (i, j) = 255, others = 0
    std::cout << "mat = \n" << mat << std::endl;
    std::cout << "dst = \n" << dst << std::endl;
    
    cv::Mat dst_1, dst_2, dst2;
    dst_1 = mat >= 90;
    dst_2 = mat <= 150;
    cv::bitwise_and(dst_1, dst_2, dst2);
    std::cout << "dst_1 = \n" << dst_1 << std::endl;
    std::cout << "dst_2 = \n" << dst_2 << std::endl;
    std::cout << "dst2 = \n" << dst2 << std::endl;
    
    cv::Mat diff = dst != dst2;
    std::vector<cv::Point> nonzeros;
    std::cout << "nonzeros = " << nonzeros << std::endl;
    */
    
    return EXIT_SUCCESS;
}
